home *** CD-ROM | disk | FTP | other *** search
- /*$T zmath.c GC 1.137 08/09/02 17:47:17 */
-
- /*
- * Some simple mathematical functions. Don't look for some logic in the function
- * names :-)
- */
- #include <stdio.h>
- #include <stdlib.h>
- #include <string.h>
- #include <math.h>
- #include "zmath.h"
-
- /* Gestion des matrices 4x4 */
- void gl_M4_Id(M4 *a) {
- a->m[0][0] = 1;
- a->m[0][1] = 0;
- a->m[0][2] = 0;
- a->m[0][3] = 0;
- a->m[1][0] = 0;
- a->m[1][1] = 1;
- a->m[1][2] = 0;
- a->m[1][3] = 0;
- a->m[2][0] = 0;
- a->m[2][1] = 0;
- a->m[2][2] = 1;
- a->m[2][3] = 0;
- a->m[3][0] = 0;
- a->m[3][1] = 0;
- a->m[3][2] = 0;
- a->m[3][3] = 1;
- }
-
- /* */
- int gl_M4_IsId(M4 *a) {
- int i, j;
- /*~~~~~*/
-
- for(i = 0; i < 4; i++) {
- for(j = 0; j < 4; j++) {
- if(i == j) {
- if(a->m[i][j] != 1.0) {
- return 0;
- }
- }
- else if(a->m[i][j] != 0.0) {
- return 0;
- }
- }
- }
-
- return 1;
- }
-
- /* */
- void gl_M4_Mul(M4 *c, M4 *a, M4 *b) {
- int i, j, k;
- float s;
- for(i = 0; i < 4; i++) {
- for(j = 0; j < 4; j++) {
- s = 0.0;
- for(k = 0; k < 4; k++) {
- s += a->m[i][k] * b->m[k][j];
- }
-
- c->m[i][j] = s;
- }
- }
- }
-
- /* c=c*a */
- void gl_M4_MulLeft(M4 *c, M4 *b) {
- int i, j, k;
- float s;
- M4 a;
-
- /* memcpy(&a, c, 16*sizeof(float)); */
- a = *c;
-
- for(i = 0; i < 4; i++) {
- for(j = 0; j < 4; j++) {
- s = 0.0;
- for(k = 0; k < 4; k++) {
- s += a.m[i][k] * b->m[k][j];
- }
-
- c->m[i][j] = s;
- }
- }
- }
-
- /* */
- void gl_M4_Move(M4 *a, M4 *b) {
- memcpy(a, b, sizeof(M4));
- }
-
- /* */
- void gl_MoveV3(V3 *a, V3 *b) {
- memcpy(a, b, sizeof(V3));
- }
-
- /* */
- void gl_MulM4V3(V3 *a, M4 *b, V3 *c) {
- a->X = b->m[0][0] * c->X + b->m[0][1] * c->Y + b->m[0][2] * c->Z + b->m[0][3];
- a->Y = b->m[1][0] * c->X + b->m[1][1] * c->Y + b->m[1][2] * c->Z + b->m[1][3];
- a->Z = b->m[2][0] * c->X + b->m[2][1] * c->Y + b->m[2][2] * c->Z + b->m[2][3];
- }
-
- /* */
- void gl_MulM3V3(V3 *a, M4 *b, V3 *c) {
- a->X = b->m[0][0] * c->X + b->m[0][1] * c->Y + b->m[0][2] * c->Z;
- a->Y = b->m[1][0] * c->X + b->m[1][1] * c->Y + b->m[1][2] * c->Z;
- a->Z = b->m[2][0] * c->X + b->m[2][1] * c->Y + b->m[2][2] * c->Z;
- }
-
- /* */
- void gl_M4_MulV4(V4 *a, M4 *b, V4 *c) {
- a->X = b->m[0][0] * c->X + b->m[0][1] * c->Y + b->m[0][2] * c->Z + b->m[0][3] * c->W;
- a->Y = b->m[1][0] * c->X + b->m[1][1] * c->Y + b->m[1][2] * c->Z + b->m[1][3] * c->W;
- a->Z = b->m[2][0] * c->X + b->m[2][1] * c->Y + b->m[2][2] * c->Z + b->m[2][3] * c->W;
- a->W = b->m[3][0] * c->X + b->m[3][1] * c->Y + b->m[3][2] * c->Z + b->m[3][3] * c->W;
- }
-
- /* transposition of a 4x4 matrix */
- void gl_M4_Transpose(M4 *a, M4 *b) {
- a->m[0][0] = b->m[0][0];
- a->m[0][1] = b->m[1][0];
- a->m[0][2] = b->m[2][0];
- a->m[0][3] = b->m[3][0];
-
- a->m[1][0] = b->m[0][1];
- a->m[1][1] = b->m[1][1];
- a->m[1][2] = b->m[2][1];
- a->m[1][3] = b->m[3][1];
-
- a->m[2][0] = b->m[0][2];
- a->m[2][1] = b->m[1][2];
- a->m[2][2] = b->m[2][2];
- a->m[2][3] = b->m[3][2];
-
- a->m[3][0] = b->m[0][3];
- a->m[3][1] = b->m[1][3];
- a->m[3][2] = b->m[2][3];
- a->m[3][3] = b->m[3][3];
- }
-
- /* inversion of an orthogonal matrix of type Y=M.X+P */
- void gl_M4_InvOrtho(M4 *a, M4 b) {
- int i, j;
- float s;
- for(i = 0; i < 3; i++) {
- for(j = 0; j < 3; j++) {
- a->m[i][j] = b.m[j][i];
- }
-
- a->m[3][0] = 0.0;
- a->m[3][1] = 0.0;
- a->m[3][2] = 0.0;
- a->m[3][3] = 1.0;
- for(i = 0; i < 3; i++) {
- s = 0;
- for(j = 0; j < 3; j++) {
- s -= b.m[j][i] * b.m[j][3];
- }
-
- a->m[i][3] = s;
- }
- }
- }
-
- /* Inversion of a general nxn matrix. Note:: m is destroyed */
- int Matrix_Inv(float *r, float *m, int n) {
- int i, j, k, l;
- float max, tmp, t;
-
- /* identitée dans r */
- for(i = 0; i < n * n; i++) {
- r[i] = 0;
- }
-
- for(i = 0; i < n; i++) {
- r[i * n + i] = 1;
- }
-
- for(j = 0; j < n; j++) {
- /* recherche du nombre de plus grand module sur la colonne j */
- max = m[j * n + j];
- k = j;
- for(i = j + 1; i < n; i++) {
- if(fabs(m[i * n + j]) > fabs(max)) {
- k = i;
- max = m[i * n + j];
- }
- }
-
- /* non intersible matrix */
- if(max == 0) {
- return 1;
- }
-
- /* permutation des lignes j et k */
- if(k != j) {
- for(i = 0; i < n; i++) {
- tmp = m[j * n + i];
- m[j * n + i] = m[k * n + i];
- m[k * n + i] = tmp;
-
- tmp = r[j * n + i];
- r[j * n + i] = r[k * n + i];
- r[k * n + i] = tmp;
- }
- }
-
- /* multiplication de la ligne j par 1/max */
- max = 1 / max;
- for(i = 0; i < n; i++) {
- m[j * n + i] *= max;
- r[j * n + i] *= max;
- }
-
- for(l = 0; l < n; l++) {
- if(l != j) {
- t = m[l * n + j];
- for(i = 0; i < n; i++) {
- m[l * n + i] -= m[j * n + i] * t;
- r[l * n + i] -= r[j * n + i] * t;
- }
- }
- }
- }
-
- return 0;
- }
-
- /* inversion of a 4x4 matrix */
- void gl_M4_Inv(M4 *a, M4 *b) {
- M4 tmp;
- memcpy(&tmp, b, 16 * sizeof(float));
-
- /* tmp=*b; */
- Matrix_Inv(&a->m[0][0], &tmp.m[0][0], 4);
- }
-
- /* */
- void gl_M4_Rotate(M4 *a, float t, int u) {
- float s, c;
- int v, w;
- if((v = u + 1) > 2) {
- v = 0;
- }
-
- if((w = v + 1) > 2) {
- w = 0;
- }
-
- s = sin(t);
- c = cos(t);
- gl_M4_Id(a);
- a->m[v][v] = c;
- a->m[v][w] = -s;
- a->m[w][v] = s;
- a->m[w][w] = c;
- }
-
- /* inverse of a 3x3 matrix */
- void gl_M3_Inv(M3 *a, M3 *m) {
- float det;
-
- det = m->m[0][0] *
- m->m[1][1] *
- m->m[2][2] -
- m->m[0][0] *
- m->m[1][2] *
- m->m[2][1] -
- m->m[1][0] *
- m->m[0][1] *
- m->m[2][2] +
- m->m[1][0] *
- m->m[0][2] *
- m->m[2][1] +
- m->m[2][0] *
- m->m[0][1] *
- m->m[1][2] -
- m->m[2][0] *
- m->m[0][2] *
- m->m[1][1];
-
- a->m[0][0] = (m->m[1][1] * m->m[2][2] - m->m[1][2] * m->m[2][1]) / det;
- a->m[0][1] = -(m->m[0][1] * m->m[2][2] - m->m[0][2] * m->m[2][1]) / det;
- a->m[0][2] = -(-m->m[0][1] * m->m[1][2] + m->m[0][2] * m->m[1][1]) / det;
-
- a->m[1][0] = -(m->m[1][0] * m->m[2][2] - m->m[1][2] * m->m[2][0]) / det;
- a->m[1][1] = (m->m[0][0] * m->m[2][2] - m->m[0][2] * m->m[2][0]) / det;
- a->m[1][2] = -(m->m[0][0] * m->m[1][2] - m->m[0][2] * m->m[1][0]) / det;
-
- a->m[2][0] = (m->m[1][0] * m->m[2][1] - m->m[1][1] * m->m[2][0]) / det;
- a->m[2][1] = -(m->m[0][0] * m->m[2][1] - m->m[0][1] * m->m[2][0]) / det;
- a->m[2][2] = (m->m[0][0] * m->m[1][1] - m->m[0][1] * m->m[1][0]) / det;
- }
-
- /* vector arithmetic */
- int gl_V3_Norm(V3 *a) {
- float n;
- n = sqrt(a->X * a->X + a->Y * a->Y + a->Z * a->Z);
- if(n == 0) {
- return 1;
- }
-
- a->X /= n;
- a->Y /= n;
- a->Z /= n;
- return 0;
- }
-
- /* */
- V3 gl_V3_New(float x, float y, float z) {
- V3 a;
- a.X = x;
- a.Y = y;
- a.Z = z;
- return a;
- }
-
- /* */
- V4 gl_V4_New(float x, float y, float z, float w) {
- V4 a;
- a.X = x;
- a.Y = y;
- a.Z = z;
- a.W = w;
- return a;
- }
-